% 占据栅格建图 + 蒙特卡洛定位

clear all;
close all;
clc;

% 添加路径
addpath(fullfile(fileparts(mfilename('fullpath')), '..', 'common'));

% 轨迹选择
fprintf('========================================\n');
fprintf('  Grid SLAM + MCL 轨迹模式选择\n');
fprintf('========================================\n');
fprintf('1. 固定路径点轨迹（可重复，适合对比）\n');
fprintf('2. 动态避障轨迹（随机探索）\n');
fprintf('========================================\n');
choice = input('请选择轨迹模式 [1/2] (默认1): ', 's');
if isempty(choice)
    choice = '1';
end

%% 加载统一配置
config = get_slam_config();

% 应用选择
if choice == '1'
    config.use_fixed_trajectory = true;
    fprintf('使用固定路径点轨迹\n\n');
elseif choice == '2'
    config.use_fixed_trajectory = false;
    fprintf('使用动态避障轨迹\n\n');
else
    fprintf('无效选择，使用默认（固定路径点）\n\n');
    config.use_fixed_trajectory = true;
end

% 解包常用参数
dt = config.dt;
total_time = config.total_time;
steps = config.steps;
Q = config.Q;
laser_range = config.laser_range;
laser_fov = config.laser_fov;
num_beams = config.num_beams;
laser_noise = config.laser_noise;
USE_FIXED_TRAJECTORY = config.use_fixed_trajectory;
initial_state = config.initial_state;

% 地图参数
map_size = 50;  % 地图尺寸 50x50m
grid_resolution = 0.2;  % 栅格分辨率 0.2m
grid_size = round(map_size / grid_resolution);  % 栅格数量

% 蒙特卡洛定位参数
N_particles = 200;  % 粒子数量
resample_threshold = 0.5;  % 重采样阈值

fprintf('占据栅格SLAM [栅格:%dx%d, 粒子:%d, 时长:%.0fs]\n', ...
        grid_size, grid_size, N_particles, total_time);

%% 生成真实环境（带障碍物）
true_obstacles = generate_obstacles();
n_obstacles = size(true_obstacles, 1);
fprintf('环境障碍物: %d个\n', n_obstacles);

%% 生成真实轨迹
if USE_FIXED_TRAJECTORY
    % 【路径点轨迹模式】
    fprintf('使用固定路径点轨迹\n');
    controls = generate_waypoint_trajectory(config.waypoints, dt, total_time, initial_state);
    
    % 使用固定控制序列仿真
    laser_params = struct('dt', dt, 'range', laser_range, 'fov', laser_fov, ...
                          'num_beams', num_beams, 'noise', laser_noise);
    noise_params = struct('Q', Q);
    [true_trajectory, ~] = simulate_with_fixed_trajectory(controls, true_obstacles, ...
                                                           laser_params, noise_params, initial_state);
else
    % 【动态避障轨迹模式】
    fprintf('使用动态避障轨迹（随机初始方向）\n');
    % 预生成整个轨迹（内部已包含随机初始方向）
    true_trajectory = generate_true_trajectory(steps, dt, config.v_base, config.omega_base, Q, true_obstacles);
    initial_state = true_trajectory(:, 1);
    fprintf('初始方向: %.2f rad (%.1f度)\n', initial_state(3), rad2deg(initial_state(3)));
end

%% 初始化
% 初始化占据栅格地图 (log-odds形式)
occupancy_map = zeros(grid_size, grid_size);  % log-odds值

% 初始化粒子
particles = initialize_mcl_particles(N_particles, initial_state', [0.5, 0.5, 0.1]);

% 数据存储
estimated_trajectory = zeros(3, steps);

%% 主循环
figure('Position', [50, 50, 1400, 700]);

progress_interval = max(1, round(steps/20));
fprintf('开始仿真...\n');

for t = 1:steps
    % 显示进度
    if mod(t, progress_interval) == 0 || t == 1 || t == steps
        percent = round(100 * t / steps);
        % 统计已建图区域
        prob_map = 1 ./ (1 + exp(-occupancy_map));
        mapped_cells = sum(prob_map(:) > 0.7 | prob_map(:) < 0.3);
        coverage = 100 * mapped_cells / (grid_size * grid_size);
        
        % 计算当前误差
        if t > 1
            current_error = norm(true_state(1:2) - estimated_trajectory(1:2, t-1));
        else
            current_error = 0;
        end
        
        fprintf('  [%3d%%] 步骤 %4d/%4d | 覆盖: %5.1f%% | 误差: %.3fm\r', ...
                percent, t, steps, coverage, current_error);
        if t == steps
            fprintf('\n');
        end
    end
    
    %% 获取真实位姿（从预生成的轨迹）
    true_state = true_trajectory(:, t);
    
    %% 计算控制输入（从真实轨迹反推，与其他算法统一）
    if t == 1
        control = [0; 0];
    else
        dx = true_trajectory(1, t) - true_trajectory(1, t-1);
        dy = true_trajectory(2, t) - true_trajectory(2, t-1);
        dtheta = wrapToPi(true_trajectory(3, t) - true_trajectory(3, t-1));
        v = sqrt(dx^2 + dy^2) / dt;
        omega = dtheta / dt;
        control = [v; omega];
    end
    
    %% 激光扫描（使用真实位姿获取观测）
    laser_scan = simulate_laser_scan(true_state, true_obstacles, laser_range, ...
                                     laser_fov, num_beams, laser_noise);
    
    %% 蒙特卡洛定位
    % 预测
    particles = mcl_predict(particles, control, dt, Q);
    
    % 更新（基于激光扫描和当前地图）
    particles = mcl_update(particles, laser_scan, occupancy_map, ...
                          laser_range, laser_fov, num_beams, grid_resolution, map_size);
    
    % 重采样
    N_eff = 1 / sum([particles.weight].^2);
    if N_eff < N_particles * resample_threshold
        particles = resample_mcl_particles(particles);
    end
    
    % 估计位姿
    estimated_state = estimate_mcl_state(particles);
    estimated_trajectory(:, t) = estimated_state;
    
    %% 更新占据栅格地图（使用估计位姿，与其他算法统一）
    occupancy_map = update_occupancy_grid(occupancy_map, estimated_state, laser_scan, ...
                                          laser_range, laser_fov, num_beams, ...
                                          grid_resolution, map_size);
    
    %% 可视化
    if mod(t, 10) == 0
        plot_grid_slam(true_state, particles, occupancy_map, true_obstacles, ...
                      true_trajectory(:, 1:t), estimated_trajectory(:, 1:t), ...
                      laser_scan, laser_range, laser_fov, num_beams, ...
                      grid_resolution, map_size, t*dt);
        drawnow;
    end
end

%% 结果统计
fprintf('\n仿真完成\n');

position_error = sqrt(sum((true_trajectory(1:2, :) - estimated_trajectory(1:2, :)).^2, 1));
fprintf('平均定位误差: %.3f m\n', mean(position_error));
fprintf('最大定位误差: %.3f m\n', max(position_error));

% 计算地图质量（log-odds转概率）
prob_map = 1 ./ (1 + exp(-occupancy_map));
occupied_cells = sum(prob_map(:) > 0.7);  % 占据
free_cells = sum(prob_map(:) < 0.3);      % 空闲
mapped_cells = occupied_cells + free_cells;
fprintf('已建图栅格: %d (%.1f%%), 占据: %d, 空闲: %d\n', ...
        mapped_cells, 100*mapped_cells/numel(occupancy_map), occupied_cells, free_cells);

%% 最终结果可视化
fprintf('\n显示最终建图结果...\n');
figure('Position', [100, 100, 1600, 700], 'Name', '栅格SLAM最终结果');

% 子图1: 占据栅格地图 + 轨迹
subplot(1, 2, 1);
hold on; axis equal;

% 显示占据栅格地图（转换为概率）
prob_map_display = 1 - 1./(1 + exp(occupancy_map));
imagesc([0, map_size], [0, map_size], prob_map_display);
colormap(gca, flipud(gray));
caxis([0, 1]);
axis xy;  % 设置坐标轴方向：x向右，y向上

% 添加颜色条
c = colorbar;
c.Label.String = '占据概率';

% 绘制真实轨迹（绿色）
plot(true_trajectory(1, :), true_trajectory(2, :), 'g-', 'LineWidth', 2, 'DisplayName', '真实轨迹');

% 绘制估计轨迹（蓝色虚线）
plot(estimated_trajectory(1, :), estimated_trajectory(2, :), 'b--', 'LineWidth', 2, 'DisplayName', '估计轨迹');

% 标记起点和终点
plot(true_trajectory(1, 1), true_trajectory(2, 1), 'ro', 'MarkerSize', 10, 'LineWidth', 2, 'DisplayName', '起点');
plot(true_trajectory(1, end), true_trajectory(2, end), 'rs', 'MarkerSize', 10, 'LineWidth', 2, 'DisplayName', '终点');

xlabel('X (m)', 'FontSize', 12);
ylabel('Y (m)', 'FontSize', 12);
title('占据栅格地图与轨迹', 'FontSize', 14, 'FontWeight', 'bold');
legend('Location', 'best');
xlim([0, map_size]); ylim([0, map_size]);
grid on;
hold off;

% 子图2: 定位误差随时间变化
subplot(1, 2, 2);
hold on; grid on;

time_axis = (1:steps) * dt;
plot(time_axis, position_error, 'b-', 'LineWidth', 1.5);
plot(time_axis, ones(size(time_axis)) * mean(position_error), 'r--', 'LineWidth', 2, ...
     'DisplayName', sprintf('平均误差: %.3f m', mean(position_error)));

xlabel('时间 (s)', 'FontSize', 12);
ylabel('定位误差 (m)', 'FontSize', 12);
title('定位误差分析', 'FontSize', 14, 'FontWeight', 'bold');
legend('定位误差', sprintf('平均误差: %.3f m', mean(position_error)), 'Location', 'best');
xlim([0, total_time]);
ylim([0, max(position_error) * 1.1]);
hold off;

% 保存结果
save('grid_slam_result.mat', 'true_trajectory', 'estimated_trajectory', ...
     'position_error', 'occupancy_map', 'particles');

fprintf('结果已保存到 grid_slam_result.mat\n');

